
//#include <sting.h>
#include "nmeap.h"
#include "nmeap_rmc.h"


#define RMC_TOKEN_ID        0
#define RMC_TOKEN_TIME      1
#define RMC_TOKEN_VALID     2
#define RMC_TOKEN_LAT       3
#define RMC_TOKEN_NS        4
#define RMC_TOKEN_LONG      5
#define RMC_TOKEN_EW        6
#define RMC_TOKEN_SPEED     7
#define RMC_TOKEN_DIR       8
#define RMC_TOKEN_DATE      9

#define MONTH_MASK          0x0f    // 1-12
#define DATE_MASK           0x1f    // 1-31
#define HOUR_MASK           0x1f    // 0-24
#define MINUTE_MASK         0x3f    // 0-59
#define SECOND_MASK         0x3f    // 0-59

#define CHAR_TO_DEC(ch)     ((ch) >= '0' && (ch) <= '9' ? (ch) - '0' : 0)

const unsigned int power_of_ten[] =
{
    1, 10, 100, 1000, 10000, 100000, 1000000
};

/** 
 * Convert string to integer, and returns the number of digits of the integer  
 *
 */
int nmeap_atoi(const char *s, unsigned char *digits)
{
    int v = 0;
    int minus = 0;
    unsigned char b = 0;

    /* space or control characters */
    while (*s == ' '  ||  (unsigned int)(*s - 9) < 5u)
    {
        s++;
    }
    switch (*s)
    {
        case '-':
            minus = 1;
            break;

        case '+':
            ++s;
            break;
    }

    /* '0' - '9' */
    while ((unsigned int)(*s - '0') < 10u)
    {
        v = v * 10 + *s - '0';
        ++s;
        ++b;
    }

    *digits = b;
    return minus ? -v : v;
}

/** 
 * standard GPRMCntence parser 
 *  
 * $GNRMC,,V,,,,,,,,,,M*4E
 * $GPRMC,061020.00,A,3456.012345,N,12345.678901,E,0.0,0.0,120620,5.0,W,A*27
 */
int nmeap_parse_rmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
{
    unsigned char minus = 0, digit;
    int n;
    char *p;

    /* get pointer to sentence data */
    nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->dataout;

    /* if there is a data element, use it */
    if (rmc != 0)
    {
        /* extract data from the tokens */
        if (*context->token[2] == 'A')
        {
            rmc->valid = 1;

            rmc->time.hour      = (CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][0]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][1])) & HOUR_MASK;
            rmc->time.minute    = (CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][2]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][3])) & MINUTE_MASK;
            rmc->time.second    = (CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][4]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][5])) & SECOND_MASK;
            rmc->time.msecond   = CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][7]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_TIME][8]);
            rmc->date.date      = (CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][0]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][1])) & DATE_MASK;
            rmc->date.month     = (CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][2]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][3])) & MONTH_MASK;
            rmc->date.year      = 2000 + CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][4]) * 10 + CHAR_TO_DEC(context->token[RMC_TOKEN_DATE][5]);

            /* latitude */
            if (context->token[RMC_TOKEN_NS][0] == 'S')
            {
                minus = 1;
            }
            n = nmeap_atoi(context->token[RMC_TOKEN_LAT], &digit);
            if (minus)
            {
                n = -n;
            }
            rmc->latitude_dm = n;

            p = strchr(context->token[RMC_TOKEN_LAT], '.');
            p++;
            n = nmeap_atoi(p, &digit);
            if (digit < 6)
            {
                n *= power_of_ten[6 - digit];
            }
            rmc->latitude_dp = n;


            /* longitude */
            if (context->token[RMC_TOKEN_EW][0] == 'W')
            {
                minus = 1;
            }
            n = nmeap_atoi(context->token[RMC_TOKEN_LONG], &digit);
            if (minus)
            {
                n = -n;
            }
            rmc->longitude_dm = n;

            p = strchr(context->token[RMC_TOKEN_LONG], '.');
            p++;
            n = nmeap_atoi(p, &digit);
            if (digit < 6)
            {
                n *= power_of_ten[6 - digit];
            }
            rmc->longitude_dp = n;
        }    
        else
        {
            rmc->valid = 0;
        }

        /* if the sentence has a callout, call it */
        if (sentence->callout != 0)
        {
            (*sentence->callout)(context, rmc, context->user_data);
        }
    }

    return NMEAP_ID_RMC;
}
